#include "motion_control.h"
#include <algorithm>
#include <math.h>
#include <iostream>
#include <cstring>

//=============================================================================
// PID控制器实现
//=============================================================================

PIDController::PIDController(float kp, float ki, float kd)
    : kp_(kp), ki_(ki), kd_(kd)
    , previous_error_(0.0f), integral_(0.0f)
    , max_integral_(1000.0f), min_output_(-1000.0f), max_output_(1000.0f)
    , proportional_(0.0f), derivative_(0.0f), first_run_(true)
{
}

void PIDController::SetParameters(float kp, float ki, float kd, float max_integral, 
                                 float min_output, float max_output)
{
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
    max_integral_ = max_integral;
    min_output_ = min_output;
    max_output_ = max_output;
}

float PIDController::Calculate(float setpoint, float measured_value, float dt)
{
    if (dt <= 0.0f) {
        return 0.0f;
    }
    
    // 计算误差
    float error = setpoint - measured_value;
    
    // 比例项
    proportional_ = kp_ * error;
    
    // 积分项 - 使用梯形积分
    integral_ += error * dt;
    // 积分限幅防止积分饱和
    integral_ = clamp(integral_, -max_integral_, max_integral_);
    float integral_term = ki_ * integral_;
    
    // 微分项 - 仅在非首次运行时计算
    derivative_ = 0.0f;
    if (!first_run_) {
        derivative_ = kd_ * (error - previous_error_) / dt;
    }
    first_run_ = false;
    
    // 更新上次误差
    previous_error_ = error;
    
    // 计算总输出
    float output = proportional_ + integral_term + derivative_;
    
    // 输出限幅
    output = clamp(output, min_output_, max_output_);
    
    return output;
}

void PIDController::Reset()
{
    previous_error_ = 0.0f;
    integral_ = 0.0f;
    proportional_ = 0.0f;
    derivative_ = 0.0f;
    first_run_ = true;
}

float PIDController::clamp(float value, float min_val, float max_val)
{
    return std::max(min_val, std::min(value, max_val));
}

//=============================================================================
// 运动控制实现
//=============================================================================

MotionControl::MotionControl()
    : usv_type_(2)
    , heading_control_active_(false)
    , velocity_control_active_(false)
    , heading_output_filter_(0.0f)
    , velocity_output_filter_(0.0f)
    , filter_alpha_(0.8f)  // 滤波系数
    , last_heading_output_(0.0f)    // 新增
    , last_velocity_output_(0.0f)   // 新增
{
    heading_pid_ = std::make_unique<PIDController>();
    velocity_pid_ = std::make_unique<PIDController>();
    
    // 设置默认PID参数
    heading_pid_->SetParameters(0.35f, 0.0f, 3.0f, 2000.0f, -30.0f, 30.0f);
    velocity_pid_->SetParameters(200.0f, 1.0f, 0.0f, 2000.0f, 10.0f, 1000.0f);
}

void MotionControl::SetController(int usv_type)
{
    usv_type_ = usv_type;
    
    // 根据USV类型调整参数
    switch (usv_type_) {
        case 1:
            // 小型USV参数
            filter_alpha_ = 0.7f;  // 响应更快
            break;
        case 2:
            // 中型USV参数  
            filter_alpha_ = 0.8f;  // 平衡
            break;
        case 3:
            // 大型USV参数
            filter_alpha_ = 0.9f;  // 更平滑
            break;
        default:
            filter_alpha_ = 0.8f;
            break;
    }
}

void MotionControl::SetHeadingPID(float kp, float ki, float kd, float max_integral, 
                                  float min_output, float max_output)
{
    heading_pid_->SetParameters(kp, ki, kd, max_integral, min_output, max_output);
}

void MotionControl::SetVelocityPID(float kp, float ki, float kd, float max_integral, 
                                   float min_output, float max_output)
{
    velocity_pid_->SetParameters(kp, ki, kd, max_integral, min_output, max_output);
}

actuator_control MotionControl::USVControl(const headvel_control& command, const TransState& state, float dt)
{
    actuator_control output;
    memset(&output, 0, sizeof(actuator_control));
    
    if (command.model == 2) { // 航向速度控制模式
        heading_control_active_ = true;
        velocity_control_active_ = true;
        
        // 航向控制
        float heading_output = HeadingControl(command.heading, static_cast<float>(state.PosEarth[3]), dt);
        
        // 速度控制 - 计算当前速度
        // float current_speed = sqrtf(state.VelEarth[0] * state.VelEarth[0] + 
                                //    state.VelEarth[1] * state.VelEarth[1]);
        float current_speed = state.VelEarth[0];
        float velocity_output = VelocityControl(command.velocity, current_speed, dt);
        
        // 应用滤波
        // heading_output_filter_ = filter_alpha_ * heading_output_filter_ + (1.0f - filter_alpha_) * heading_output;
        // velocity_output_filter_ = filter_alpha_ * velocity_output_filter_ + (1.0f - filter_alpha_) * velocity_output;
        
        // 映射到执行器
        output = MapControlToActuator(heading_output, velocity_output);
    } else {
        heading_control_active_ = false;
        velocity_control_active_ = false;
    }
    
    return output;
}

float MotionControl::HeadingControl(float target_heading, float current_heading, float dt)
{
    // 计算航向误差 (考虑角度环绕)
    float heading_error = CalculateHeadingError(target_heading, current_heading);
    
    // PID控制 - 设定值为0，误差作为测量值的负值
    float control_output = heading_pid_->Calculate(0.0f, -heading_error, dt);
    
    last_heading_output_ = control_output;  // 存储输出
    return control_output;
}

float MotionControl::VelocityControl(float target_velocity, float current_velocity, float dt)
{
    // 确保目标速度为正值
    target_velocity = std::max(0.0f, target_velocity);
    
    float control_output = velocity_pid_->Calculate(target_velocity, current_velocity, dt);
    
    last_velocity_output_ = control_output;  // 存储输出
    return control_output;
}

void MotionControl::Reset()
{
    heading_pid_->Reset();
    velocity_pid_->Reset();
    heading_output_filter_ = 0.0f;
    velocity_output_filter_ = 0.0f;
    heading_control_active_ = false;
    velocity_control_active_ = false;
    last_heading_output_ = 0.0f;
    last_velocity_output_ = 0.0f;
}

float MotionControl::NormalizeAngle(float angle)
{
    while (angle > 180.0f) angle -= 360.0f;
    while (angle < -180.0f) angle += 360.0f;
    return angle;
}

float MotionControl::CalculateHeadingError(float target, float current)
{
    // 归一化角度到[-180, 180]
    target = NormalizeAngle(target);
    current = NormalizeAngle(current);
    
    // 计算最短角度差
    float error = target - current;
    error = NormalizeAngle(error);
    
    return error;
}

actuator_control MotionControl::MapControlToActuator(float heading_output, float velocity_output)
{
    actuator_control output;
    memset(&output, 0, sizeof(actuator_control));
    
    // 确保输出在合理范围内
    heading_output = std::max(-30.0f, std::min(30.0f, heading_output));
    velocity_output = std::max(0.0f, std::min(1000.0f, velocity_output));
    
    switch (usv_type_) {
        case 1: // 单舵单电机
            output.steer_l = heading_output;
            output.steer_r = heading_output;
            output.motor_l = velocity_output;
            output.motor_r = velocity_output;
            break;
            
        case 2: // 差动推进 (左右电机独立控制)
            {
                // 根据航向输出调整左右电机转速
                float heading_factor = heading_output / 30.0f; // 归一化到[-1, 1]
                
                output.motor_l = velocity_output * (1.0f - 0.5f * heading_factor);
                output.motor_r = velocity_output * (1.0f + 0.5f * heading_factor);
                
                // 确保电机转速不为负
                output.motor_l = std::max(0.0f, output.motor_l);
                output.motor_r = std::max(0.0f, output.motor_r);
                
                // 舵角保持中立
                output.steer_l = 0.0f;
                output.steer_r = 0.0f;
            }
            break;
            
        case 3: // 全向推进
            output.steer_l = heading_output;
            output.steer_r = heading_output;
            output.motor_l = velocity_output;
            output.motor_r = velocity_output;
            output.bucket_l = 0.0f;  // 推进器角度
            output.bucket_r = 0.0f;
            break;
            
        default: // 默认配置
            output.steer_l = heading_output;
            output.steer_r = heading_output;
            output.motor_l = velocity_output;
            output.motor_r = velocity_output;
            break;
    }
    
    return output;
}